#include "../User/Task/CMD.h"

#include <stdint.h>

#include "../User/Device/CMD.h"
#include "cmsis_os.h"
#include "main.h"
#include "sys.h"

/* USER CODE BEGIN Header_Task_CMD */
/**
 * @brief  Function implementing the TaskCMD thread.
 * @param  argument: Not used
 * @retval None
 */
/* USER CODE END Header_Task_CMD */
void Task_CMD(void *argument) {
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK_CMD_SCAN_FREQ;

  CMD_Init(&(Task_Runtime.cmd_send));

  uint32_t tick = osKernelGetTickCount();

  while (1) {
    tick += delay_tick;

    if (CMD_Update(&(Task_Runtime.cmd_send))) {
      CMD_Apply(&Task_Runtime.cmd_send, &(Task_Runtime.pid.angle),
                &(Task_Runtime.pid.speed));
    }

    osDelayUntil(tick);
  }
}
